/* **********************************************************
 * Copyright 2010 Dimitar Vangelovski
 *
 * This file is part of the Gens3 3D Engine.
 *
 * The Gens3 3D Engine is free software; you can redistribute it and/or
 * modify it under the terms of version 2 of the GNU General Public
 * License as published by the Free Software Foundation.
 *
 * The Gens3 3D Engine is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with the Gens3 3D Engine; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301, USA
 * **********************************************************/


#pragma once

#include "common.h"

#define PI          3.1415926535897932
#define BEHIND		0
#define INTERSECTS	1
#define FRONT		2
//#define in(a)		((unsigned int &) a)

#ifdef _WIN32
#define __isnan__	_isnan
#else
#define __isnan__	isnan
#endif

const float		MaxSteepness = 60.0f;   // Max steepness the player can climb in degrees

class CVector
{
public:

	CVector();
	CVector(const CVector &other);
	CVector(float x2, float y2, float z2);
	~CVector();

	inline CVector operator + (const CVector &vVec) { return CVector(vVec.x + x, vVec.y + y, vVec.z + z); }
	inline CVector operator - (const CVector &vVec) { return CVector(x - vVec.x, y - vVec.y, z - vVec.z); }
	inline CVector operator * (float fValue) { return CVector(x * fValue, y * fValue, z * fValue); }
	inline CVector operator / (float fValue) { return CVector(x / fValue, y / fValue, z / fValue); }

public:
	float x, y, z;
};


/////////////////////////////////////////////////////////////////////////////////////////////////
// common 3D math functions

// convert degrees to radians
inline float	Deg2Rad(float fDeg) { return fDeg * PI / 180.0f; }

// convert radians to degrees
inline float	Rad2Deg(float fRad) { return fRad * 180.0f / PI; }

// calculate the absolute value
inline float	Absolute(float fValue) { return (fValue < 0) ? -fValue : fValue; }

// calculate the magnitude of a vector
inline float	Magnitude(CVector vNormal) { return (float)sqrt( vNormal.x * vNormal.x + vNormal.y * vNormal.y + vNormal.z * vNormal.z ); }

// calculate the distance the plane is from the origin (0, 0, 0)
inline float	PlaneDistance(CVector Normal, CVector Point) { return -(Normal.x * Point.x + Normal.y * Point.y + Normal.z * Point.z); }

// calculate the dot product of 2 vectors
inline float	Dot(CVector vVector1, CVector vVector2) { return ( vVector1.x * vVector2.x + vVector1.y * vVector2.y + vVector1.z * vVector2.z ); }

// calculate the cross product of two vectors
inline CVector	Cross(CVector vVector1, CVector vVector2)
{
	return CVector( (vVector1.y * vVector2.z - vVector1.z * vVector2.y),
					(vVector1.z * vVector2.x - vVector1.x * vVector2.z),
					(vVector1.x * vVector2.y - vVector1.y * vVector2.x) );
}

// normalize a vector
inline CVector	Normalize(CVector vNormal)
{
	float fMagnitude = Magnitude(vNormal);

	vNormal.x /= fMagnitude;
	vNormal.y /= fMagnitude;
	vNormal.z /= fMagnitude;

	return vNormal;
}

// calculate the normal of the polygon
inline CVector	Normal(CVector vPolygon[])
{
	CVector vVector1 = vPolygon[2] - vPolygon[0];
	CVector vVector2 = vPolygon[1] - vPolygon[0];

	// calculate the cross product to get a perpendicular vector (the normal)
	CVector vNormal = Cross(vVector1, vVector2);

	vNormal = Normalize(vNormal);

	return vNormal;
}

// test if the polygon is below and facing the player sphere
// it takes the normal of the intersecting polygon and computes
// the angle of the normal and a vertical vector
inline bool	Check_if_on_Ground(CVector TestNormal)
{
        CVector VertVec;
	float DotCos;

	VertVec.y = -1.0f;
	DotCos = VertVec.y * TestNormal.y;
	if (DotCos > cos(MaxSteepness * PI / 180.0f)) return true;

	return false;
}

// calculate the distance between two points in space
inline float	Distance(CVector vPoint1, CVector vPoint2)
{
	return (float)sqrt( (vPoint2.x - vPoint1.x) * (vPoint2.x - vPoint1.x) +
					    (vPoint2.y - vPoint1.y) * (vPoint2.y - vPoint1.y) +
					    (vPoint2.z - vPoint1.z) * (vPoint2.z - vPoint1.z) );
}

// calculate the angle between 2 vectors
inline double	AngleBetweenVectors(CVector Vector1, CVector Vector2)
{
	// based on the mathematics formula for dot product:
	//		DotProd = |A| * |B| * cos(t)
	// =>	cos(t) = DotProd / (|A| * |B|)

	float dotProduct = Dot(Vector1, Vector2);

	float vectorsMagnitude = Magnitude(Vector1) * Magnitude(Vector2);

	double angle = acos( dotProduct / vectorsMagnitude );

	// if the angle is -1.#IND0000000 number (indefinite) => return 0
	if (__isnan__(angle)) return 0.0f;

	return angle;
}

// test if the intersection point is inside the polygon
inline bool	InsidePolygon(CVector vIntersection, CVector Poly[], int verticeCount)
{
	const double MATCH_FACTOR = 0.99;			// Used to cover up the error in floating point
	double Angle = 0.0;
	CVector vA, vB;

	// Go in a circle to each vertex and get the angle between
	for (int i = 0; i < verticeCount; i++)
	{
		vA = Poly[i] - vIntersection;			// Subtract the intersection point from the current vertex

		vB = Poly[(i + 1) % verticeCount] - vIntersection;	// Subtract the point from the next vertex

		Angle += AngleBetweenVectors(vA, vB);	// Find the angle between the 2 vectors and add them all up as we go along
	}

	if (Angle >= (MATCH_FACTOR * (2.0 * PI)) )	// If the angle is greater than 2 PI, (360 degrees)
		return true;							// => The point is inside the polygon

	return false;								// if we get here, the point is outside the polygon
}

// test if the intersection point lies inside a triangle
// optimized version of the above function when we have only triangles
inline bool	InsideTriangle(CVector vIntersection, CVector Poly[])
{
	CVector vA = Poly[0] - vIntersection;
	CVector vB = Poly[1] - vIntersection;
	CVector vC = Poly[2] - vIntersection;

	if((AngleBetweenVectors(vA, vB) +
		AngleBetweenVectors(vB, vC) +
		AngleBetweenVectors(vC, vA)) >= (0.99 * (2.0 * PI))) return true;

	return false;
}

//// test if the intersection point lies inside a triangle (faster than the above function)
//inline bool	InsideTriangle(CVector vPoint, CVector vPoly[])
//{
//	CVector e10 = vPoly[1] - vPoly[0];
//	CVector e20 = vPoly[2] - vPoly[0];
//
//	float a = Dot(e10, e10);
//	float b = Dot(e10, e20);
//	float c = Dot(e20, e20);
//	float ac_bb = (a * c) - (b * b);
//
//	CVector vp(vPoint.x - vPoly[0].x, vPoint.y - vPoly[0].y, vPoint.z - vPoly[0].z);
//
//	float d = Dot(vp, e10);
//	float e = Dot(vp, e20);
//	float x = (d * c) - (e * b);
//	float y = (e * a) - (d * b);
//	float z = x + y - ac_bb;
//
//	return (( in(z) & ~(in(x) | in(y)) ) & 0x80000000);
//}

// calculate a bounce-back vector to move back the sphere so a collision is not happening
// this is for collision with the inside of the polygon, not it's edges
inline CVector	GetCollisionOffset(CVector &vNormal, float radius, float distance)
{
	CVector vOffset;

	// collision with the front of the polygon
	if (distance > 0)
	{
		float distanceOver = radius - distance;
		vOffset = vNormal * distanceOver;
	}
	else // colliding from behind the polygon
	{
		float distanceOver = radius + distance;
		vOffset = vNormal * -distanceOver;
	}

	return vOffset;
}

// classifies a sphere according to a plane (BEHIND, in FRONT, or INTERSECTS)
inline int	ClassifySphere(CVector &vCenter, CVector &vNormal, CVector &vPoint, float radius, float &distance)
{
	// calculate the distance our polygon plane is from the origin
	float d = PlaneDistance(vNormal, vPoint);

	// calculate the distance the center point of the sphere is from the polygon plane
	distance = vNormal.x * vCenter.x + vNormal.y * vCenter.y + vNormal.z * vCenter.z + d;

	if (Absolute(distance) < radius)
	{
		return INTERSECTS;
	}
	else if (distance >= radius)
	{
		return FRONT;
	}

	return BEHIND;
}

/////////////////////////////////////////////////////////////////////////////////////////////////
// matrix operations

// construct a translation matrix based on a (x,y,z) vector
inline void	TranslationMatrix(float x, float y, float z, float m[])
{
	m[0] = 1.0f;  m[4] = 0.0f;  m[8]  = 0.0f;  m[12] = x;

	m[1] = 0.0f;  m[5] = 1.0f;  m[9]  = 0.0f;  m[13] = y;

	m[2] = 0.0f;  m[6] = 0.0f;  m[10] = 1.0f;  m[14] = z;

	m[3] = 0.0f;  m[7] = 0.0f;  m[11] = 0.0f;  m[15] = 1.0f;
}

// construct a rotation matrix for rotation around the X axis for an angle x
inline void	RotationMatrix_X(float x, float m[])
{
	m[0] = 1.0f;  m[4] = 0.0f;     m[8]  = 0.0f;    m[12] = 0.0f;

	m[1] = 0.0f;  m[5] = cos(x);   m[9]  = sin(x);  m[13] = 0.0f;

	m[2] = 0.0f;  m[6] = -sin(x);  m[10] = cos(x);  m[14] = 0.0f;

	m[3] = 0.0f;  m[7] = 0.0f;     m[11] = 0.0f;    m[15] = 1.0f;
}

// construct a rotation matrix for rotation around the Y axis for an angle y
inline void	RotationMatrix_Y(float y, float m[])
{
	m[0] = cos(y);  m[4] = 0.0f;  m[8]  = -sin(y);  m[12] = 0.0f;

	m[1] = 0.0f;    m[5] = 1.0f;  m[9]  = 0.0f;     m[13] = 0.0f;

	m[2] = sin(y);  m[6] = 0.0f;  m[10] = cos(y);   m[14] = 0.0f;

	m[3] = 0.0f;    m[7] = 0.0f;  m[11] = 0.0f;     m[15] = 1.0f;
}

// construct a rotation matrix for rotation around the Z axis for an angle z
inline void	RotationMatrix_Z(float z, float m[])
{
	m[0] = cos(z);   m[4] = sin(z);  m[8]  = 0.0f;  m[12] = 0.0f;

	m[1] = -sin(z);  m[5] = cos(z);  m[9]  = 0.0f;  m[13] = 0.0f;

	m[2] = 0.0f;     m[6] = 0.0f;    m[10] = 1.0f;  m[14] = 0.0f;

	m[3] = 0.0f;     m[7] = 0.0f;    m[11] = 0.0f;  m[15] = 1.0f;
}

// multiply vector vi (vector input) with matrix m and create vo (vector output)
inline void	VectorMatrixMult(float vi[], float m[], float vo[])
{
	vo[0] = m[0] * vi[0] + m[1] * vi[1] + m[2] * vi[2] + m[3] * vi[3];
	vo[1] = m[4] * vi[0] + m[5] * vi[1] + m[6] * vi[2] + m[7] * vi[3];
	vo[2] = m[8] * vi[0] + m[9] * vi[1] + m[10]* vi[2] + m[11]* vi[3];
	vo[3] = m[12]* vi[0] + m[13]* vi[1] + m[14]* vi[2] + m[15]* vi[3];
}

// multiply matrix m1 with matrix m2 and create matrix mo
inline void	MatrixMatrixMult(float m1[], float m2[], float mo[])
{
	mo[0]  = m1[0] * m2[0]  + m1[4] * m2[1]  + m1[8]  * m2[2]  + m1[12] * m2[3];
	mo[1]  = m1[1] * m2[0]  + m1[5] * m2[1]  + m1[9]  * m2[2]  + m1[13] * m2[3];
	mo[2]  = m1[2] * m2[0]  + m1[6] * m2[1]  + m1[10] * m2[2]  + m1[14] * m2[3];
	mo[3]  = m1[3] * m2[0]  + m1[7] * m2[1]  + m1[11] * m2[2]  + m1[15] * m2[3];
	////////////////////////////////////////////////////////////////////////////////
	mo[4]  = m1[0] * m2[4]  + m1[4] * m2[5]  + m1[8]  * m2[6]  + m1[12] * m2[7];
	mo[5]  = m1[1] * m2[4]  + m1[5] * m2[5]  + m1[9]  * m2[6]  + m1[13] * m2[7];
	mo[6]  = m1[2] * m2[4]  + m1[6] * m2[5]  + m1[10] * m2[6]  + m1[14] * m2[7];
	mo[7]  = m1[3] * m2[4]  + m1[7] * m2[5]  + m1[11] * m2[6]  + m1[15] * m2[7];
	////////////////////////////////////////////////////////////////////////////////
	mo[8]  = m1[0] * m2[8]  + m1[4] * m2[9]  + m1[8]  * m2[10] + m1[12] * m2[11];
	mo[9]  = m1[1] * m2[8]  + m1[5] * m2[9]  + m1[9]  * m2[10] + m1[13] * m2[11];
	mo[10] = m1[2] * m2[8]  + m1[6] * m2[9]  + m1[10] * m2[10] + m1[14] * m2[11];
	mo[11] = m1[3] * m2[8]  + m1[7] * m2[9]  + m1[11] * m2[10] + m1[15] * m2[11];
	////////////////////////////////////////////////////////////////////////////////
	mo[12] = m1[0] * m2[12] + m1[4] * m2[13] + m1[8]  * m2[14] + m1[12] * m2[15];
	mo[13] = m1[1] * m2[12] + m1[5] * m2[13] + m1[9]  * m2[14] + m1[13] * m2[15];
	mo[14] = m1[2] * m2[12] + m1[6] * m2[13] + m1[10] * m2[14] + m1[14] * m2[15];
	mo[15] = m1[3] * m2[12] + m1[7] * m2[13] + m1[11] * m2[14] + m1[15] * m2[15];
}

////////////////////////////////////////////////////////////////////////////////////////////////////

// rotate a point around another point (rotation center) using matrix transformations
CVector		RotatePointAroundPoint_Matrix(CVector vOldPoint, CVector vRotCenter, CVector vRotAngle);

// rotate a point around another point (rotation center)
CVector		RotatePointAroundPoint(CVector vOldPoint, CVector vRotCenter, CVector vRotAngle);

// calculate closest point on a line segment A-B to the point vPoint
CVector		ClosestPointOnLine(CVector vA, CVector vB, CVector vPoint);

// test a collision with the polygon edges, and calculate a bounce-back vector
bool		EdgeSphereCollision(CVector &vCenter, CVector vPolygon[], int vertexCount, float radius, CVector &vEdgeBounce, CVector &vEdgeNormal);

